/* USER CODE BEGIN Header */
/**
  ******************************************************************************
  * @file           : main.c
  * @brief          : Main program body
  ******************************************************************************
  * @attention
  *
  * Copyright (c) 2025 STMicroelectronics.
  * All rights reserved.
  *
  * This software is licensed under terms that can be found in the LICENSE file
  * in the root directory of this software component.
  * If no LICENSE file comes with this software, it is provided AS-IS.
  *
  ******************************************************************************
  */
/* USER CODE END Header */
/* Includes ------------------------------------------------------------------*/
#include "main.h"
#include "can.h"
#include "gpio.h"

/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include "pid.h"
/* USER CODE END Includes */

/* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN PTD */

/* USER CODE END PTD */

/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */

/* USER CODE END PD */

/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM */

/* USER CODE END PM */

/* Private variables ---------------------------------------------------------*/

/* USER CODE BEGIN PV */

//---------------can相关初始化-----------------
CAN_TxHeaderTypeDef Tx_can_1;
CAN_RxHeaderTypeDef Rx_Can_1;
uint32_t mail;
uint8_t send_content[8]={0};
typedef struct
{
	uint16_t can_id;			//can ID
	int16_t set_voltage;		//电压？
	int16_t rotor_angle;		//电机角度，虽然收报是uint但是你处理的时候可以加点料让它变成int
	int16_t rotor_speed;		//电机速度
	int16_t torque_current;		//电机电流
	uint8_t temp;				//电机温度
}moto_info_t;
moto_info_t rx_Motor_message;	//新建实例
//---------------can相关初始化-----------------
//---------------pid相关初始化-----------------
#define Target_Angle (float)0.0		//目标角度
#define Target_rpm (float)600.0		//目标转速
pid_TypeDef motor_pid;				//初始化pid结构体
float error_angle = Target_Angle;		//声明一个误差变量，初始化为目标角度
float j = 0;
int monitor_PID;					//监控变量
double monitor_PID_error_integral;	//监控变量
float i_limit = 200;
//---------------pid相关初始化-----------------

/* USER CODE END PV */

/* Private function prototypes -----------------------------------------------*/
void SystemClock_Config(void);
/* USER CODE BEGIN PFP */

/* USER CODE END PFP */

/* Private user code ---------------------------------------------------------*/
/* USER CODE BEGIN 0 */
//------------------------------------------------------------------------------------------------------
// 函数简介     can发送函数
// 使用示例     To_can(error);
// 参数介绍		input				电机某参数目标值与实际值的差
// 返回参数     void
// 备注信息
//------------------------------------------------------------------------------------------------------
void To_can(int16_t input) {
	//初始化can的具体信息
	Tx_can_1.StdId = 0x1ff;
	Tx_can_1.ExtId = 0;
	Tx_can_1.IDE = CAN_ID_STD;
	Tx_can_1.RTR = CAN_RTR_DATA;
	Tx_can_1.DLC = 8;
	Tx_can_1.TransmitGlobalTime = DISABLE;
	
	int16_t data = (int16_t)(pid_calculate(&motor_pid, input) * (float)25000);	//得到pid计算结果
	monitor_PID = data;											//监视
	monitor_PID_error_integral = motor_pid.error_integral;		//监视
	
	send_content[0] = (data & 0xff00) >> 8;		//发送pid嚼过的数据
	send_content[1] = (data & 0x00ff);
	
	HAL_CAN_AddTxMessage(&hcan,&Tx_can_1,send_content,&mail);
}

void can_filter_init(void)
{
	CAN_FilterTypeDef can_filter_st;
	can_filter_st.FilterActivation = ENABLE;
	can_filter_st.FilterMode = CAN_FILTERMODE_IDMASK;
	can_filter_st.FilterScale = CAN_FILTERSCALE_32BIT;
	can_filter_st.FilterIdHigh = 0x0000;
	can_filter_st.FilterIdLow = 0x0000;
	can_filter_st.FilterMaskIdHigh = 0x0000;
	can_filter_st.FilterMaskIdLow = 0x0000;
	can_filter_st.FilterBank = 0;
	can_filter_st.FilterFIFOAssignment = CAN_FILTER_FIFO0;
	can_filter_st.SlaveStartFilterBank = 14;
	
	HAL_CAN_ConfigFilter(&hcan, &can_filter_st);
	HAL_CAN_ActivateNotification(&hcan, CAN_IT_RX_FIFO0_MSG_PENDING);
	HAL_CAN_Start(&hcan);
	
}
/* USER CODE END 0 */

/**
  * @brief  The application entry point.
  * @retval int
  */
int main(void)
{

  /* USER CODE BEGIN 1 */
  /* USER CODE END 1 */

  /* MCU Configuration--------------------------------------------------------*/

  /* Reset of all peripherals, Initializes the Flash interface and the Systick. */
  HAL_Init();

  /* USER CODE BEGIN Init */

  /* USER CODE END Init */

  /* Configure the system clock */
  SystemClock_Config();

  /* USER CODE BEGIN SysInit */

  /* USER CODE END SysInit */

  /* Initialize all configured peripherals */
  MX_GPIO_Init();
  MX_CAN_Init();
  /* USER CODE BEGIN 2 */
	HAL_CAN_Start(&hcan);
	can_filter_init();
	pid_init(&motor_pid, 0.0005, 0.00005, 0, 1, 500, 1);	//注意积分限幅需要使用原始数据，需要和output同单位
	//pid_init(&motor_pid, 0.05, 0.0000, 0, 1, 500, 0);

  /* USER CODE END 2 */

  /* Infinite loop */
  /* USER CODE BEGIN WHILE */
	
  while (1)
  {
		if (rx_Motor_message.rotor_angle> Target_Angle+4096) rx_Motor_message.rotor_angle -= 8192;	//猜一下这个是干啥的
		error_angle = Target_Angle + j - rx_Motor_message.rotor_angle;		//循环获取当前角度误差
		//error = Target_rpm - rx_Motor_message.rotor_speed;			//循环获取当前转速误差
		To_can(error_angle);
		HAL_Delay(1);
    /* USER CODE END WHILE */

    /* USER CODE BEGIN 3 */
  }
  /* USER CODE END 3 */
}

/**
  * @brief System Clock Configuration
  * @retval None
  */
void SystemClock_Config(void)
{
  RCC_OscInitTypeDef RCC_OscInitStruct = {0};
  RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};

  /** Initializes the RCC Oscillators according to the specified parameters
  * in the RCC_OscInitTypeDef structure.
  */
  RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSE;
  RCC_OscInitStruct.HSEState = RCC_HSE_ON;
  RCC_OscInitStruct.HSEPredivValue = RCC_HSE_PREDIV_DIV1;
  RCC_OscInitStruct.HSIState = RCC_HSI_ON;
  RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
  RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE;
  RCC_OscInitStruct.PLL.PLLMUL = RCC_PLL_MUL9;
  if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK)
  {
    Error_Handler();
  }

  /** Initializes the CPU, AHB and APB buses clocks
  */
  RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK
                              |RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2;
  RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
  RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
  RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV2;
  RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1;

  if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_2) != HAL_OK)
  {
    Error_Handler();
  }
}

/* USER CODE BEGIN 4 */


void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan) {
	uint8_t rx_data[8] = {0,0,0,0,0,0,0,0};
	if (hcan->Instance == CAN1) {
		if(HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO0,&Rx_Can_1, rx_data) == HAL_OK) {
			rx_Motor_message.can_id = Rx_Can_1.StdId;
			rx_Motor_message.rotor_angle = (uint16_t)rx_data[0]<<8 | (uint16_t)rx_data[1];
			rx_Motor_message.rotor_speed = (uint16_t)rx_data[2]<<8 | (uint16_t)rx_data[3];
			rx_Motor_message.torque_current = (uint16_t)rx_data[4]<<8 | (uint16_t)rx_data[5];
			rx_Motor_message.temp = (uint16_t)rx_data[6];
			if(Rx_Can_1.StdId == 0x250) {
				//To_can();
			}
		}
	}
}

/* USER CODE END 4 */

/**
  * @brief  This function is executed in case of error occurrence.
  * @retval None
  */
void Error_Handler(void)
{
  /* USER CODE BEGIN Error_Handler_Debug */
  /* User can add his own implementation to report the HAL error return state */
  __disable_irq();
  while (1)
  {
  }
  /* USER CODE END Error_Handler_Debug */
}

#ifdef  USE_FULL_ASSERT
/**
  * @brief  Reports the name of the source file and the source line number
  *         where the assert_param error has occurred.
  * @param  file: pointer to the source file name
  * @param  line: assert_param error line source number
  * @retval None
  */
void assert_failed(uint8_t *file, uint32_t line)
{
  /* USER CODE BEGIN 6 */
  /* User can add his own implementation to report the file name and line number,
     ex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */
  /* USER CODE END 6 */
}
#endif /* USE_FULL_ASSERT */
